/**************************************************************
 * obstacle_avoidance.c
 * 
 *   实时避障算法实现 目前还没调试使用
 * 
 *  Created on: 2024-01-20
 *      Author: robot
 *************************************************************/

#include "obstacle_avoidance.h"
#include <stdlib.h>

// 参数配置
#define OBSTACLE_HISTORY_SIZE 5
#define MIN_INTENSITY_THRESHOLD 15
#define MAX_VALID_DISTANCE 5000
#define HYSTERESIS_THRESHOLD 50

/**
 * @brief 初始化避障滤波器
 */
static void InitObstacleFilter(ObstacleAvoidanceFilter *filter)
{
    for (int i = 0; i < OBSTACLE_HISTORY_SIZE; i++) {
        filter->front_distance_history[i] = 0;
    }
    filter->history_index = 0;
    filter->valid_count = 0;
    filter->current_safe_distance = 0;
}

/**
 * @brief 计算前方扇形区域的最小有效距离
 */
static uint16_t CalculateFrontMinDistance(LiDARFrameTypeDef *frame)
{
    uint16_t min_distance = UINT16_MAX;
    uint8_t valid_points = 0;
    
    // 计算每个点的角度
    float angle_step = (frame->end_angle - frame->start_angle) / (POINT_PER_PACK - 1);
    
    for (int i = 0; i < POINT_PER_PACK; i++) {
        float current_angle = (frame->start_angle + angle_step * i) / 100.0f; // 转换为度
        
        // 只关注前方±45度区域
        if (current_angle >= -45.0f && current_angle <= 45.0f) {
            // 有效数据判断
            if (frame->point[i].distance > 0 && 
                frame->point[i].distance < MAX_VALID_DISTANCE &&
                frame->point[i].confidence >= MIN_INTENSITY_THRESHOLD) {
                
                if (frame->point[i].distance < min_distance) {
                    min_distance = frame->point[i].distance;
                }
                valid_points++;
            }
        }
    }
    
    return (valid_points > 0) ? min_distance : 0;
}

/**
 * @brief 更新前方障碍物距离
 */
static uint16_t UpdateFrontObstacleDistance(ObstacleAvoidanceFilter *filter, LiDARFrameTypeDef *frame)
{
    // 1. 获取前方最小距离
    uint16_t front_min = CalculateFrontMinDistance(frame);
    
    if (front_min == 0 || front_min == UINT16_MAX) {
        // 无有效数据，保持上次值
        return (filter->valid_count > 0) ? filter->current_safe_distance : 0;
    }
    
    // 2. 更新滑动窗口
    filter->front_distance_history[filter->history_index] = front_min;
    filter->history_index = (filter->history_index + 1) % OBSTACLE_HISTORY_SIZE;
    
    if (filter->valid_count < OBSTACLE_HISTORY_SIZE) {
        filter->valid_count++;
    }
    
    // 3. 计算滑动窗口内的最小值
    uint16_t window_min = UINT16_MAX;
    for (int i = 0; i < filter->valid_count; i++) {
        if (filter->front_distance_history[i] < window_min && 
            filter->front_distance_history[i] > 0) {
            window_min = filter->front_distance_history[i];
        }
    }
    
    // 4. 应用滞后滤波，避免频繁切换
    if (window_min != UINT16_MAX) {
        int16_t diff = abs((int16_t)window_min - (int16_t)filter->current_safe_distance);
        if (diff > HYSTERESIS_THRESHOLD || filter->current_safe_distance == 0) {
            filter->current_safe_distance = window_min;
        }
    }
    
    return filter->current_safe_distance;
}

/**
 * @brief 多方向障碍物检测
 */
static void DetectMultiDirectionObstacles(LiDARFrameTypeDef *frame, MultiDirectionObstacle *result)
{
    // 初始化
    for (int i = 0; i < DIRECTION_COUNT; i++) {
        result->min_distances[i] = UINT16_MAX;
        result->obstacle_flags[i] = 0;
    }
    
    float angle_step = (frame->end_angle - frame->start_angle) / (POINT_PER_PACK - 1);
    
    for (int i = 0; i < POINT_PER_PACK; i++) {
        float current_angle = (frame->start_angle + angle_step * i) / 100.0f;
        uint16_t distance = frame->point[i].distance;
        uint8_t intensity = frame->point[i].confidence;
        
        // 数据有效性检查
        if (distance == 0 || distance > MAX_VALID_DISTANCE || intensity < MIN_INTENSITY_THRESHOLD) {
            continue;
        }
        
        // 前方区域 (-30° to 30°)
        if (current_angle >= -30.0f && current_angle <= 30.0f) {
            if (distance < result->min_distances[DIRECTION_FRONT]) {
                result->min_distances[DIRECTION_FRONT] = distance;
            }
        }
        // 左侧区域 (-90° to -30°)
        else if (current_angle >= -90.0f && current_angle < -30.0f) {
            if (distance < result->min_distances[DIRECTION_LEFT]) {
                result->min_distances[DIRECTION_LEFT] = distance;
            }
        }
        // 右侧区域 (30° to 90°)
        else if (current_angle > 30.0f && current_angle <= 90.0f) {
            if (distance < result->min_distances[DIRECTION_RIGHT]) {
                result->min_distances[DIRECTION_RIGHT] = distance;
            }
        }
    }
    
    // 处理无效值
    for (int i = 0; i < DIRECTION_COUNT; i++) {
        if (result->min_distances[i] == UINT16_MAX) {
            result->min_distances[i] = 0;
        }
    }
}

// ================== 公共函数实现 ==================

/**
 * @brief 初始化避障系统
 */
void InitObstacleSystem(ObstacleAvoidanceSystem *system, uint16_t emergency_distance, uint16_t safe_distance)
{
    InitObstacleFilter(&system->front_filter);
    system->last_update_time = 0;
    system->emergency_stop_distance = emergency_distance;
    system->safe_distance = safe_distance;
    
    // 初始化多方向检测结果
    for (int i = 0; i < DIRECTION_COUNT; i++) {
        system->current_obstacles.min_distances[i] = 0;
        system->current_obstacles.obstacle_flags[i] = 0;
    }
}

/**
 * @brief 主避障决策函数
 */
ObstacleAction ProcessObstacleAvoidance(ObstacleAvoidanceSystem *system, LiDARFrameTypeDef *frame)
{
    // 1. 更新前方障碍物距离
    uint16_t front_distance = UpdateFrontObstacleDistance(&system->front_filter, frame);
    
    // 2. 多方向检测
    DetectMultiDirectionObstacles(frame, &system->current_obstacles);
    
    // 3. 更新障碍物标志
    for (int i = 0; i < DIRECTION_COUNT; i++) {
        system->current_obstacles.obstacle_flags[i] = 
            (system->current_obstacles.min_distances[i] > 0 && 
             system->current_obstacles.min_distances[i] < system->safe_distance) ? 1 : 0;
    }
    
    // 4. 决策逻辑
    if (front_distance > 0 && front_distance < system->emergency_stop_distance) {
        return ACTION_EMERGENCY_STOP;
    }
    else if (front_distance > 0 && front_distance < system->safe_distance) {
        // 前方有障碍物，检查左右选择较安全方向
        if (!system->current_obstacles.obstacle_flags[DIRECTION_LEFT] && 
            !system->current_obstacles.obstacle_flags[DIRECTION_RIGHT]) {
            // 左右都无障碍，随机选择
            return (rand() % 2) ? ACTION_TURN_LEFT : ACTION_TURN_RIGHT;
        }
        else if (!system->current_obstacles.obstacle_flags[DIRECTION_LEFT]) {
            return ACTION_TURN_LEFT;
        }
        else if (!system->current_obstacles.obstacle_flags[DIRECTION_RIGHT]) {
            return ACTION_TURN_RIGHT;
        }
        else {
            // 左右都有障碍，紧急停止
            return ACTION_EMERGENCY_STOP;
        }
    }
    else if (system->current_obstacles.obstacle_flags[DIRECTION_LEFT] && 
             !system->current_obstacles.obstacle_flags[DIRECTION_RIGHT]) {
        return ACTION_SLIGHT_RIGHT;
    }
    else if (system->current_obstacles.obstacle_flags[DIRECTION_RIGHT] && 
             !system->current_obstacles.obstacle_flags[DIRECTION_LEFT]) {
        return ACTION_SLIGHT_LEFT;
    }
    
    return ACTION_MOVE_FORWARD;
}

/**
 * @brief 获取前方障碍物距离
 */
uint16_t GetFrontObstacleDistance(ObstacleAvoidanceSystem *system)
{
    return system->front_filter.current_safe_distance;
}

/**
 * @brief 获取多方向障碍物距离
 */
void GetMultiDirectionDistances(ObstacleAvoidanceSystem *system, uint16_t *front, uint16_t *left, uint16_t *right)
{
    if (front) *front = system->current_obstacles.min_distances[DIRECTION_FRONT];
    if (left) *left = system->current_obstacles.min_distances[DIRECTION_LEFT];
    if (right) *right = system->current_obstacles.min_distances[DIRECTION_RIGHT];
}